﻿#include "robotarm.h"
#include <qmath.h>
#include <QParallelAnimationGroup>
#include <QPropertyAnimation>

RobotArm::RobotArm(Qt3DCore::QEntity* parent)
	: Qt3DCore::QEntity(parent)
{
	// 创建基座
	Joint base;
	base.entity = new Qt3DCore::QEntity(this);
	base.transform = new Qt3DCore::QTransform;
	base.length = 0.5f;

	auto* baseMesh = new Qt3DExtras::QCylinderMesh;
	baseMesh->setRadius(0.3f);
	baseMesh->setLength(base.length);

	auto* material = new Qt3DExtras::QPhongMaterial;
	material->setDiffuse(QColor(QRgb(0x808080)));

	base.entity->addComponent(baseMesh);
	base.entity->addComponent(material);
	base.entity->addComponent(base.transform);

	joints.append(base);

	// 添加关节（示例为3自由度）
	for (int i = 0; i < 3; ++i) {
		Joint joint;
		joint.entity = new Qt3DCore::QEntity(this);
		joint.transform = new Qt3DCore::QTransform;
		joint.length = 1.0f + i * 0.2f; // 不同长度的关节

		auto* mesh = new Qt3DExtras::QCylinderMesh;
		mesh->setRadius(0.1f);
		mesh->setLength(joint.length);

		// 设置初始位置（连接到前一个关节末端）
		QVector3D pos = joints.last().transform->translation();
		pos.setY(pos.y() + joints.last().length / 2);
		joint.transform->setTranslation(pos);

		joint.entity->addComponent(mesh);
		joint.entity->addComponent(new Qt3DExtras::QPhongMaterial);
		joint.entity->addComponent(joint.transform);

		joints.append(joint);
	}
}

void RobotArm::solveIK(const QVector3D& target) {
	const float tolerance = 0.01f;
	const int maxIterations = 100;
	const float stepSize = 0.1f;

	for (int iter = 0; iter < maxIterations; ++iter) {
		// 从末端向基座迭代
		for (int i = joints.size() - 1; i >= 1; --i) {
			// 获取当前关节位置
			QVector3D jointPos = joints[i].transform->translation();

			// 计算末端位置
			QVector3D endEffector = joints.last().transform->translation();
			endEffector.setY(endEffector.y() + joints.last().length / 2);

			// 计算关节到末端和关节到目标的向量
			QVector3D toEnd = endEffector - jointPos;
			QVector3D toTarget = target - jointPos;

			// 计算旋转角度（绕Z轴）
			float angle = acos(QVector3D::dotProduct(toEnd.normalized(),
				toTarget.normalized()));

			// 限制最大旋转步长
			angle = qMin(angle, stepSize);

			// 计算旋转轴（使用叉积确定方向）
			QVector3D rotAxis = QVector3D::crossProduct(toEnd, toTarget).normalized();

			// 更新关节角度
			joints[i].angle += qRadiansToDegrees(angle) * (rotAxis.z() > 0 ? 1 : -1);

			// 应用旋转
			QQuaternion rot = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), joints[i].angle);
			joints[i].transform->setRotation(rot);

			// 更新子关节位置
			for (int j = i + 1; j < joints.size(); j++) {
				QVector3D newPos = joints[j - 1].transform->translation();
				newPos.setY(newPos.y() + joints[j - 1].length / 2);
				joints[j].transform->setTranslation(newPos);
			}

			// 检查是否到达目标
			if ((endEffector - target).length() < tolerance)
				return;
		}
	}
}

void RobotArm::moveToPosition(const QVector3D& target) {
	// 使用逆运动学计算关节角度
	solveIK(target);

	// 可选：添加动画过渡
	QParallelAnimationGroup* group = new QParallelAnimationGroup;

	for (int i = 1; i < joints.size(); i++) {
		QPropertyAnimation* anim = new QPropertyAnimation(joints[i].transform, "rotation");
		anim->setDuration(1000);
		anim->setStartValue(joints[i].transform->rotation());

		// 计算目标旋转
		QQuaternion targetRot = QQuaternion::fromAxisAndAngle(
			QVector3D(0, 0, 1),
			joints[i].angle
		);
		anim->setEndValue(targetRot);
		group->addAnimation(anim);
	}

	group->start();
}


